#include "corner_harris.h"

#include "blur_gaussian.h"
#include "conversion_grayscale.h"
#include "edge_sobel.h"

CornerHarris::CornerHarris(PNM* img) :
	Convolution(img)
{
}

CornerHarris::CornerHarris(PNM* img, ImageViewer* iv) :
	Convolution(img, iv)
{
}

PNM* CornerHarris::transform()
{
	int    threshold    = getParameter("threshold").toInt();
	double sigma        = getParameter("sigma").toDouble(),
		sigma_weight = getParameter("sigma_weight").toDouble(),
		k_param      = getParameter("k").toDouble();

	int width  = image->width(),
		height = image->height();

	PNM* newImage = new PNM(width, height, QImage::Format_Mono);

	math::matrix<double> I_x2 = math::matrix<double>(width, height);
	math::matrix<double> I_y2 = math::matrix<double>(width, height);
	math::matrix<double> I_xy = math::matrix<double>(width, height);
	math::matrix<double> corners_candidates = math::matrix<double>(width, height);
	math::matrix<double> corners_nonmax_suppress = math::matrix<double>(width, height);

	PNM* tempimage;

	auto grayscale = new ConversionGrayscale(image);
	tempimage = grayscale->transform();

	auto gauss = new BlurGaussian(tempimage);
	tempimage = gauss->transformForParameters(3, 3.6f);

	auto sobel = new EdgeSobel(tempimage);
	auto x_gradient = sobel->rawHorizontalDetection();
	auto y_gradient = sobel->rawVerticalDetection();

	for (int i = 0; i < width; i++)
	{
		for (int j = 0; j < height; j++)
		{
			auto gx_pix = (*x_gradient)(i, j);
			auto gy_pix = (*y_gradient)(i, j);

			I_x2(i, j) = gx_pix * gx_pix;
			I_y2(i, j) = gy_pix * gy_pix;
			I_xy(i, j) = gx_pix * gy_pix;

			//qDebug() << I_x2(i, j) << " | " << I_y2(i, j) << " | " << I_xy(i, j);
		}
	}

	for (int i = 0; i < width; i++)
	{
		for (int j = 0; j < height; j++)
		{
			if((i == 0 || i == width - 1 || j == 0 || j == height - 1))
			{
				corners_candidates(i, j) = 0;
				corners_nonmax_suppress(i, j) = 0;
			}
			else 
			{
				double S_x2 = 0.0f;
				double S_y2 = 0.0f;
				double S_xy = 0.0f;

				for (int k = -1; k <= 1; k++)
				{
					for (int l = -1; l <= 1; l++)
					{
						double g_val = BlurGaussian::getGauss(k, l, sigma);

						S_x2 += I_x2(i+k, j+l) * g_val;
						S_y2 += I_y2(i+k, j+l) * g_val;
						S_xy += I_xy(i+k, j+l) * g_val;
					}
				}

				S_x2 /= sigma_weight;
				S_y2 /= sigma_weight;
				S_xy /= sigma_weight;

				math::matrix<double> H = math::matrix<double>(2, 2);
				H(0,0) = S_x2; H(1,1) = S_y2; H(0,1) = S_xy; H(1,0) = S_xy;

				double r = H(0,0)*H(1,1) - H(0,1)*H(1,0) - k_param*pow(H(0,0) + H(1,1), 2);

				if(r > threshold) 
					corners_candidates(i, j) = r;
				else 
					corners_candidates(i, j) = 0;
			}
		}
	}

	bool make_nonmax_suppression = true;

	while (make_nonmax_suppression)
	{
		make_nonmax_suppression = false;
		for (int i = 1; i < width-1; i++)
		{
			for (int j = 1; j < height-1; j++)
			{
				auto val = corners_candidates(i, j);

				bool ismax = true;
				for (int k = -1; k <= 1; k++)
					for (int l = -1; l <= 1; l++)
						if(corners_candidates(i + k, j + l) > val) 
							ismax = false;

				if(ismax)
				{
					corners_nonmax_suppress(i, j) = val;
				}
				else
				{
					if(val > 0) make_nonmax_suppression = true;
					corners_nonmax_suppress(i, j) = 0;
				}
			}
		}

		corners_candidates = corners_nonmax_suppress;
	}

	for (int i = 0; i < width; i++)
	{
		for (int j = 0; j < height; j++)
		{
			if(corners_candidates(i, j) == 0)
				newImage->setPixel(i, j, 1);
			else 
				newImage->setPixel(i, j, 0);
		}
	}

	return newImage;
}
